#include "ModelMD5Animation.h"
#include "../Logger.h"
#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>

#include "../SOIL/SOIL.h"
namespace fs = boost::filesystem;

#define GLM_SWIZZLE_XYZW
#include "../glm/glm.hpp"
#include "../glm/gtc/type_ptr.hpp"
#include "../glm/gtc/swizzle.hpp"
#include "../glm/gtc/quaternion.hpp"
#include "../glm/gtx/vector_angle.hpp"
#include "../glm/gtx/compatibility.hpp"
#include "../glm/gtx/matrix_operation.hpp"
#include "../glm/gtx/transform.hpp"
#include "../glm/gtx/quaternion.hpp"
#include "../glm/gtx/euler_angles.hpp"

_MD5Animation::_MD5Animation(){}
_MD5Animation::~_MD5Animation(){}
bool _MD5Animation::load(const char *filename){
    if ( !fs::exists(filename) )
    {
        errLog.print(Str()<< "MD5Animation::load: Failed to find file: " << filename);
        return false;
    }
	
    fs::path filePath = filename;
	
    std::string param;
    std::string junk;   // Read junk from the file
	
    fs::ifstream file(filename);
    int fileLength = getFileLength(file);
    if (fileLength == 0)
	{
		errLog.print(Str()<< "MD5Animation::load: Failed to find file: " << filename << "file size = 0");
        return false;
	}
	
    jointInfos.clear();
    bounds.clear();
    baseFrames.clear();
    frames.clear();
    animatedSkeleton.joints.clear();
    iNumFrames = 0;
	file >> param;
	
	while(!file.eof())
	{
		if (param == "MD5Version")
		{
			file >> iMD5Version;
			if (iMD5Version != 10)
			{
				errLog.print(Str()<<"Could not load the animation file: "<<filename<<", the version incorrect: "<<iMD5Version<<" (only version 10 is supported)");
			}
		}
		else if (param == "commandline")
		{
			file.ignore( fileLength, '\n' ); // Ignore everything else on the line
		}
		else if (param == "numFrames")
		{
			file >> iNumFrames;
			file.ignore( fileLength, '\n' );
		}
		else if (param == "numJoints")
		{
			file >> iNumJoints;
			file.ignore( fileLength, '\n' );
		}
		else if (param == "frameRate")
		{
			file >> iFramRate;
			file.ignore(fileLength, '\n');
		}
		else if (param == "numAnimatedComponents")
		{
			file >> iNumAnimatedComponents;
			file.ignore(fileLength, '\n');
		}
		else if (param == "hierarchy")
		{
			file >> junk; // read in the '{' character
			for (int i = 0; i < iNumJoints; i++)
			{
				JointInfo joint;
				file
				>> joint.name
				>> joint.parentID
				>> joint.flags
				>> joint.startIndex;
				removeQuotes(joint.name);
				
				jointInfos.push_back(joint);
				
				file.ignore(fileLength, '\n');
			}
			file >> junk; // read in the '}' character
		}
		else if (param == "bounds")
		{
			file >> junk; // read in the '{' character
			file.ignore( fileLength, '\n' );
			for (int i = 0; i < iNumFrames; i++)
			{
				Bound bound;
				file >> junk; // read in the '(' character
				file
				>> bound.min.x
				>> bound.min.y
				>> bound.min.z;
				file
				>> junk
				>> junk; // read in the ')' and '(' characters.
				file
				>> bound.max.x
				>> bound.max.y
				>> bound.max.z;
				
				bounds.push_back(bound);
				
				file.ignore(fileLength, '\n');
			}
			
			file >> junk; // read in the '}' character
			file.ignore( fileLength, '\n' );
		}
		else if (param == "baseframe")
		{
			file >> junk; // read in the '{' character
			file.ignore( fileLength, '\n' );
			
			for (int i = 0; i < iNumJoints; i++)
			{
				BaseFrame baseFrame;
				file >> junk;
				file >> baseFrame.pos.x >> baseFrame.pos.y >> baseFrame.pos.z;
				file >> junk >> junk;
				file >> baseFrame.orient.x >> baseFrame.orient.y >> baseFrame.orient.z;
				file.ignore( fileLength, '\n' );
				
				baseFrames.push_back(baseFrame);
			}
			file >> junk; // read in the '}' character
			file.ignore( fileLength, '\n' );
		}
		else if (param == "frame")
		{
			FrameData frame;
			file >> frame.iFrameID >> junk; // Read in the '{' character
			file.ignore(fileLength, '\n' );
			
			for (int i = 0; i < iNumAnimatedComponents; i++)
			{
				float frameData;
				file >> frameData;
				frame.frameData.push_back(frameData);
			}
			
			frames.push_back(frame);
			
			// Build a skeleton for this frame
			buildFrameSkeleton( skeletons, jointInfos, baseFrames, frame );
			
			file >> junk; // Read in the '}' character
			file.ignore(fileLength, '\n' );
		}
		
		file >> param;
	} // while ( !file.eof )
	// Make sure there are enough joints for the animated skeleton.
    animatedSkeleton.joints.assign(iNumJoints, SkeletonJoint() );
	
    fFrameDuration = 1.0f / (float)iFramRate;
    fAnimDuration = ( fFrameDuration * (float)iNumFrames );
    fAnimTime = 0.0f;
	
    assert( (int)jointInfos.size() == iNumJoints );
    assert( (int)bounds.size() == iNumFrames );
    assert( (int)baseFrames.size() == iNumJoints );
    assert( (int)frames.size() == iNumFrames );
    assert( (int)skeletons.size() == iNumFrames );
	
    return true;
}
void _MD5Animation::buildFrameSkeleton( FrameSkeletonList& skeletons, const JointInfoList& jointInfos, const BaseFrameList& baseFrames, const FrameData& frameData ){
    FrameSkeleton skeleton;
	
    for ( unsigned int i = 0; i < jointInfos.size(); i++ )
    {
        unsigned int j = 0;
		
        const JointInfo& jointInfo = jointInfos[i];
        // Start with the base frame position and orientation.
        SkeletonJoint animatedJoint = baseFrames[i];
		
        animatedJoint.parent = jointInfo.parentID;
		
        if ( jointInfo.flags & 1 ) // Pos.x
        {
            animatedJoint.pos.x = frameData.frameData[ jointInfo.startIndex + j++ ];
        }
        if ( jointInfo.flags & 2 ) // Pos.y
        {
            animatedJoint.pos.y = frameData.frameData[ jointInfo.startIndex + j++ ];
        }
        if ( jointInfo.flags & 4 ) // Pos.x
        {
            animatedJoint.pos.z  = frameData.frameData[ jointInfo.startIndex + j++ ];
        }
        if ( jointInfo.flags & 8 ) // Orient.x
        {
            animatedJoint.orient.x = frameData.frameData[ jointInfo.startIndex + j++ ];
        }
        if ( jointInfo.flags & 16 ) // Orient.y
        {
            animatedJoint.orient.y = frameData.frameData[ jointInfo.startIndex + j++ ];
        }
        if ( jointInfo.flags & 32 ) // Orient.z
        {
            animatedJoint.orient.z = frameData.frameData[ jointInfo.startIndex + j++ ];
        }
		computeQuatW( animatedJoint.orient );
		
        if ( animatedJoint.parent >= 0 ) // Has a parent joint
        {
            SkeletonJoint& parentJoint = skeleton.joints[animatedJoint.parent];
            glm::vec3 rotPos = parentJoint.orient * animatedJoint.pos;
			
            animatedJoint.pos = parentJoint.pos + rotPos;
            animatedJoint.orient = parentJoint.orient * animatedJoint.orient;
			
            animatedJoint.orient = glm::normalize( animatedJoint.orient );
        }
		
        skeleton.joints.push_back(animatedJoint);
    }
	
    skeletons.push_back(skeleton);
}
void _MD5Animation::update( float fDeltaTime ){
    if ( iNumFrames < 1 ) return;
	
    fAnimTime += fDeltaTime;
	
    while ( fAnimTime > fAnimDuration ) fAnimTime -= fAnimDuration;
    while ( fAnimTime < 0.0f ) fAnimTime += fAnimDuration;
	
    // Figure out which frame we're on
    float fFramNum = fAnimTime * (float)iFramRate;
    int iFrame0 = (int)floorf( fFramNum );
    int iFrame1 = (int)ceilf( fFramNum );
    iFrame0 = iFrame0 % iNumFrames;
    iFrame1 = iFrame1 % iNumFrames;
	
    float fInterpolate = fmodf( fAnimTime, fFrameDuration ) / fFrameDuration;
	
    interpolateSkeletons( animatedSkeleton, skeletons[iFrame0], skeletons[iFrame1], fInterpolate );
}
void _MD5Animation::interpolateSkeletons( FrameSkeleton& finalSkeleton, const FrameSkeleton& skeleton0, const FrameSkeleton& skeleton1, float fInterpolate ){
    for ( int i = 0; i < iNumJoints; i++ )
    {
        SkeletonJoint& finalJoint = finalSkeleton.joints[i];
        const SkeletonJoint& joint0 = skeleton0.joints[i];
        const SkeletonJoint& joint1 = skeleton1.joints[i];
		
        finalJoint.parent = joint0.parent;
		
        finalJoint.pos = glm::lerp( joint0.pos, joint1.pos, fInterpolate );
        finalJoint.orient = glm::mix( joint0.orient, joint1.orient, fInterpolate );
    }
}
// Get's the size of the file in bytes.
int _MD5Animation::getFileLength(std::istream &file){
    int pos = (int)file.tellg();
    file.seekg(0, std::ios::end );
    int length = (int)file.tellg();
    // Restore the position of the get pointer
    file.seekg(pos);
	
    return length;
}
// Remove the quotes from a string
void _MD5Animation::removeQuotes(std::string &str){
    size_t n;
    while ( ( n = str.find('\"') ) != std::string::npos ) str.erase(n,1);
}
void _MD5Animation::computeQuatW(glm::quat& quat){
    float t = 1.0f - ( quat.x * quat.x ) - ( quat.y * quat.y ) - ( quat.z * quat.z );
    if ( t < 0.0f )
    {
        quat.w = 0.0f;
    }
    else
    {
        quat.w = -sqrtf(t);
    }
}
